#include "MeEigenMath.h"

using namespace std;

Eigen::Vector3d MeEigenMath::mean(std::vector<Eigen::Vector3d> &pts)
{
    Eigen::Vector3d mean = {0, 0, 0};
    if (pts.size() == 0) {
        return mean;
    }
    for (uint i=0; i< pts.size(); i++) {
        mean += pts[i];
    }
    mean /= pts.size();
    return mean;
}

double MeEigenMath::distance(const Eigen::Vector3d &a, const Eigen::Vector3d &b)
{
    return sqrt((b[0]-a[0])*(b[0]-a[0]) + (b[1]-a[1])*(b[1]-a[1]) + (b[2]-a[2])*(b[2]-a[2]));
}

Eigen::Vector3d MeEigenMath::mePointToVector3d(MePoint3D &pt)
{
    return Eigen::Vector3d(pt.v);
}

MePoint3D MeEigenMath::vector3dToMePoint(Eigen::Vector3d &vec)
{
    return MePoint3D(vec.data());
}

std::vector<MePoint3D> MeEigenMath::vector3dsToMePoints(std::vector<Eigen::Vector3d> &ptsV3)
{
    vector<MePoint3D> pts;
    for (uint i=0; i< ptsV3.size(); i++) {
        pts.push_back(MePoint3D(ptsV3[i].data()));
    }
    return pts;
}

std::vector<Eigen::Vector3d> MeEigenMath::mePointsToVector3ds(std::vector<MePoint3D> &pts)
{
    vector<Eigen::Vector3d> ptsV3;
    for (uint i=0; i< pts.size(); i++) {
        ptsV3.push_back(Eigen::Vector3d(pts[i].data()));
    }
    return ptsV3;
}
#ifndef ME_NO_CV
MePoint3D MeEigenMath::cvPoint3fToMePoint(cv::Point3f point)
{
    return MePoint3D(point.x, point.y, point.z);
}

std::vector<Eigen::Vector3d> MeEigenMath::cvPointsToVector3ds(std::vector<cv::Point3d> &pts)
{
    vector<Eigen::Vector3d> ptsV3;
    for (uint i=0; i< pts.size(); i++) {
        ptsV3.push_back(Eigen::Vector3d(pts[i].x, pts[i].y, pts[i].z));
    }
    return ptsV3;
}

void MeEigenMath::cvPointsToVector3ds(std::vector<cv::Point3d> &pts, std::vector<Eigen::Vector3d> &ptsV3)
{
    ptsV3.clear();
    for (uint i=0; i< pts.size(); i++) {
        ptsV3.push_back(Eigen::Vector3d(pts[i].x, pts[i].y, pts[i].z));
    }
}

std::vector<MePoint3D> MeEigenMath::cvPointsToMePoints(std::vector<cv::Point3d> &pts)
{
    vector<MePoint3D> ptsMe;
    for (uint i=0; i< pts.size(); i++) {
        ptsMe.push_back(MePoint3D(pts[i].x, pts[i].y, pts[i].z));
    }
    return ptsMe;
}

std::vector<cv::Point3d> MeEigenMath::vector3dsToCvPoints(std::vector<Eigen::Vector3d> &pts)
{
    vector<cv::Point3d> cvpts;
    for (uint i=0; i< pts.size(); i++) {
        cvpts.push_back({pts[i][0], pts[i][1], pts[i][2]});
    }
    return cvpts;
}

std::vector<cv::Point3d> MeEigenMath::mePointsToCvPoints(std::vector<MePoint3D> &pts)
{
    vector<cv::Point3d> cvpts;
    for (uint i=0; i< pts.size(); i++) {
        cvpts.push_back({pts[i][0], pts[i][1], pts[i][2]});
    }
    return cvpts;
}

std::vector<cv::Point2f> MeEigenMath::cvKeyPointsToPoints(const std::vector<cv::KeyPoint> &kpts)
{
    vector<cv::Point2f> cvpts;
    for (uint i=0; i< kpts.size(); i++) {
        cvpts.push_back(kpts[i].pt);
    }
    return cvpts;
}
#endif // ME_NO_CV

Matrix4d MeEigenMath::rotMatToMatrix4d(Eigen::Matrix3d &rotaionMatrix, Eigen::Vector3d &t)
{
    Matrix4d m;
    for(uint r=0; r<3; ++r ) {
        for(uint c=0; c<3; ++c ) {
            m(r,c) = rotaionMatrix(r,c);
        }
    }
    m[0][3] = t[0];
    m[1][3] = t[1];
    m[2][3] = t[2];

    m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
    return m;
}

Matrix4d MeEigenMath::rotMatToMatrix4d(Eigen::Matrix3d &rotaionMatrix)
{
    Matrix4d m;
    for(uint r=0; r<3; ++r ) {
        for(uint c=0; c<3; ++c ) {
            m(r,c) = rotaionMatrix(r,c);
        }
    }
    m[0][3] = 0;
    m[1][3] = 0;
    m[2][3] = 0;

    m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
    return m;
}

Eigen::Matrix3d MeEigenMath::matrix4dToRotMat(Matrix4d &m, Eigen::Vector3d *t)
{
    Eigen::Matrix3d rotaionMatrix;
    for(uint r=0; r<3; ++r ) {
        for(uint c=0; c<3; ++c ) {
            rotaionMatrix(r,c) = m(r,c);
        }
    }

    if (t) {
        (*t)[0] = m(3,0);
        (*t)[1] = m(3,1);
        (*t)[2] = m(3,2);
    }
    return rotaionMatrix;
}

Eigen::Isometry3d MeEigenMath::matrxi4dToIsometry3d(Matrix4d &m)
{
    Eigen::Isometry3d t;
    for (int r=0; r<4; r++) {
        for(int c=0; c<4; c++) {
            t(r,c) = m(r,c);
        }
    }
    return t;
}

Eigen::Vector3d MeEigenMath::matrix4d2EigenVector(Matrix4d &m)
{
    Eigen::Matrix3d rotaionMatrix;
    for(uint r=0; r<3; ++r ) {
        for(uint c=0; c<3; ++c ) {
            rotaionMatrix(r,c) = m(r,c);
        }
    }
    Eigen::AngleAxisd rotVec(rotaionMatrix);

    return rotVec.axis() * rotVec.angle();
}

Eigen::Vector3d MeEigenMath::rotationMatrix2EigenVector(Eigen::Matrix3d &rotationMatrix)
{
    Eigen::AngleAxisd rotVec(rotationMatrix);

    return rotVec.axis() * rotVec.angle();
}

Eigen::Vector3d MeEigenMath::quaternionToEigenVector(double w, double x, double y, double z)
{
    Eigen::Quaterniond quaternion(w,x,y,z);
    Eigen::AngleAxisd rotVec(quaternion);
    return rotVec.axis() * rotVec.angle();
}

void MeEigenMath::rotationVector2Matrix(Eigen::Vector3d &rotaionVector, Eigen::Matrix3d &rotaionMatrix)
{
    const double eps =1e-8;
    double rad = rotaionVector.norm();
    if (rad < eps) {
        rotaionMatrix << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    } else {
        Eigen::Vector3d axisVec = rotaionVector/rad;
        Eigen::AngleAxisd angleAxis(rad, axisVec);
        rotaionMatrix = angleAxis.toRotationMatrix();
    }
}

Eigen::Quaterniond MeEigenMath::rotationVector2Quaternion(Eigen::Vector3d &rotationVector)
{
    const double eps =1e-8;
    double rad = rotationVector.norm();
    Eigen::Quaterniond q;
    if (rad < eps) {
        return Eigen::Quaterniond(1, 0, 0, 0);

    } else {
        Eigen::Vector3d axisVec = rotationVector/rad;
        Eigen::AngleAxisd angleAxis(rad, axisVec);
        return Eigen::Quaterniond(angleAxis);
    }
}

Eigen::Vector3d MeEigenMath::multiplyVector3d(Matrix4d &m, Eigen::Vector3d &vec)
{
    Eigen::Vector3d out;
    MeEigenMath::multiplyVector3d(m, vec, out);
    return out;
}

Eigen::Vector3d MeEigenMath::multiplyVector3d(Matrix4d &m, const Eigen::Vector3d &vec)
{
    Eigen::Vector3d out;
    MeEigenMath::multiplyVector3d(m, vec, out);
    return out;
}

void MeEigenMath::multiplyVector3d(Matrix4d &m, Eigen::Vector3d &vec, Eigen::Vector3d &outP)
{
    double v1 = vec[0];
    double v2 = vec[1];
    double v3 = vec[2];
    double v4 = 1;

    outP[0] = v1*m[0][0] + v2*m[0][1] + v3*m[0][2] + v4*m[0][3];
    outP[1] = v1*m[1][0] + v2*m[1][1] + v3*m[1][2] + v4*m[1][3];
    outP[2] = v1*m[2][0] + v2*m[2][1] + v3*m[2][2] + v4*m[2][3];
}

void MeEigenMath::multiplyVector3d(Matrix4d &m, const Eigen::Vector3d &vec, Eigen::Vector3d &outP)
{
    double v1 = vec[0];
    double v2 = vec[1];
    double v3 = vec[2];
    double v4 = 1;

    outP[0] = v1*m[0][0] + v2*m[0][1] + v3*m[0][2] + v4*m[0][3];
    outP[1] = v1*m[1][0] + v2*m[1][1] + v3*m[1][2] + v4*m[1][3];
    outP[2] = v1*m[2][0] + v2*m[2][1] + v3*m[2][2] + v4*m[2][3];
}

void MeEigenMath::calPointsDistance(std::vector<MePoint3D> &points, std::vector<double> &dis)
{
    dis.clear();
    if (points.size() < 2) {return;}
    for(uint i=0; i< points.size()-1; i++) {
        for (uint j=i+1; j < points.size(); j++) {
            dis.push_back(MeMath::distance(points[i].data(), points[j].data()));
        }
    }
}

void MeEigenMath::calPointsDistance(std::vector<Eigen::Vector3d> &points, std::vector<double> &dis)
{
    dis.clear();
    if (points.size() < 2) {return;}
    for(uint i=0; i< points.size()-1; i++) {
        for (uint j=i+1; j < points.size(); j++) {
            dis.push_back(MeMath::distance(points[i].data(), points[j].data()));
        }
    }
}

void MeEigenMath::calPointsDistance(std::vector<cv::Point3d> &points, std::vector<double> &dis)
{
    dis.clear();
    if (points.size() < 2) {return;}
    for(uint i=0; i< points.size()-1; i++) {
        for (uint j=i+1; j < points.size(); j++) {
            dis.push_back(distanceFrom2p(points[i], points[j]));
        }
    }
}

void MeEigenMath::calPointOnLine(Eigen::Vector3d &p1, Eigen::Vector3d &p2, double offsetToP1, Eigen::Vector3d &resultP)
{
    Eigen::Vector3d vDir = p2 - p1;
    vDir.normalize();
    resultP = p1 + vDir * offsetToP1;
}

int MeEigenMath::meanStdDevPoints(const std::vector<Eigen::Vector3d> &points, Eigen::Vector3d &meanP, double &mean, double &rms,
                                  double &maxV)
{
    uint count = points.size();
    if (count == 0) {
        return -1;
    }

    double scale = 1. / (count);

    meanP = {0,0,0};
    foreach (const Eigen::Vector3d &p, points) {
        meanP += p;
    }
    meanP *= scale;

    double sum = { 0. }, sqsum = { 0. };
    maxV = 0;
    for (unsigned int i = 0; i < count; ++i) {
        double v = MeEigenMath::distance(points[i], meanP);
        sum += v;
        sqsum += v * v;
        if (v > maxV) {
            maxV = v;
        }
    }
    mean = sum * scale;

    // 所有距离平方/n 开方
    double variance = std::max(sqsum*scale, 0.);
    rms = std::sqrt(variance);
    return 0;
}

bool MeEigenMath::intersectionLines(Eigen::Vector3d &l0, Eigen::Vector3d &l1, Eigen::Vector3d &m0, Eigen::Vector3d &m1, std::vector<Eigen::Vector3d> &outClosestPts, Eigen::Vector3d &meanClosestPt)
{
    Eigen::Vector3d u = { l1[0]-l0[0], l1[1]-l0[1], l1[2]-l0[2] };
    Eigen::Vector3d v = { m1[0]-m0[0], m1[1]-m0[1], m1[2]-m0[2] };
    Eigen::Vector3d w = { l0[0]-m0[0], l0[1]-m0[1], l0[2]-m0[2] };

    if(abs(u.dot(v) / u.norm() / v.norm() - 1.0) < 1e-7) {
        meanClosestPt[0] = 0;
        meanClosestPt[1] = 0;
        meanClosestPt[2] = 0;
        return false; // 平行
    }
    int nGroup=2;
    Eigen::MatrixXd A((nGroup - 1)*3, 2);
    Eigen::VectorXd b((nGroup - 1)*3);

    uint i;
    for (i=0; i < 3; i++) {
        A(i,0) = u[i];
        A(i,1) = -v[i];
        b[i] = -w[i];
    }

    Eigen::Vector2d t = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    outClosestPts.resize(2);
    outClosestPts[0] = l0 + t[0] * u;
    outClosestPts[1] = m0 + t[1] * v;
    meanClosestPt = (outClosestPts[0] + outClosestPts[1])/2;

    return true;
}

#ifndef ME_NO_VTK
void MeEigenMath::matrixToVtkMatrix(Matrix4d &m, vtkSmartPointer<vtkMatrix4x4> &vtkMatrix)
{
    for(uint i =0; i< 4; i++) {
        for(uint j=0; j<4; j++) {
            vtkMatrix->SetElement(i,j, m(i,j));
        }
    }
}

void MeEigenMath::matrixToVtkMatrix(Matrix4d &m, vtkMatrix4x4 &vtkMatrix)
{
    for(uint i =0; i< 4; i++) {
        for(uint j=0; j<4; j++) {
            vtkMatrix.SetElement(i,j, m(i,j));
        }
    }
}
#endif // ME_NO_VTK

